#include "hal_i2c.h"
#include <stdbool.h>
#include <stdint.h>
#include <hal_sensor.h>

#define ICM20789_BARO_MEASURE_TP_LP    0x609C
#define ICM20789_BARO_MEASURE_TP_N     0x6825
#define ICM20789_BARO_MEASURE_TP_LN    0x70DF
#define ICM20789_BARO_MEASURE_TP_ULN   0x7866

#define ICM20789_BARO_MEASURE_PT_LP    0x401A
#define ICM20789_BARO_MEASURE_PT_N     0x48A3
#define ICM20789_BARO_MEASURE_PT_LN    0x5059
#define ICM20789_BARO_MEASURE_PT_ULN   0x58E0

#define ICM20789_BARO_RESET            0x805D
#define ICM20789_BARO_READ_ID          0xEFC8

struct icm20789_baro_param_s{
	float sensor_constants[4];
	float p_Pa_calib[3];
	float LUT_Lower;
	float LUT_Upper;
	float quadr_factor;
	float offst_factor;
};

int32_t icm20789_i2c_send_cmd(struct hal_dev_s *dev,uint16_t cmd)
{
	struct hal_i2c_msg_s msg;

	uint8_t buf[2];
	
	buf[0] = (uint8_t)(cmd / 256);
	buf[1] = (uint8_t)(cmd % 256);
	
	msg.buff   = buf;
	msg.length = 2;
	msg.flags  = HAL_I2C_WR;

	if(hal_i2c_transfer(dev,&msg,1) > 0){
		return 1;
	}
	return 0;
}

int32_t icm20789_i2c_read_param(struct hal_dev_s *dev)
{
	struct hal_i2c_msg_s msg;
	struct hal_i2c_dev_s *i2c_dev = (struct hal_i2c_dev_s *)dev;
	struct icm20789_baro_param_s *param = (struct icm20789_baro_param_s *)i2c_dev->user_data;
	int i;
	short temp;

	uint8_t buf[5];
	
	buf[0] = 0xC5;
	buf[1] = 0x95;
	buf[2] = 0x00;
	buf[3] = 0x66;
	buf[4] = 0x9C;
	
	msg.buff   = buf;
	msg.length = 5;
	msg.flags  = HAL_I2C_WR;

	if(hal_i2c_transfer(dev,&msg,1) < 0){
		return 1;
	}
	
	for(i = 0;i < 4;i++){
		buf[0] = 0xC7;
		buf[1] = 0xF7;
		msg.buff   = buf;
		msg.length = 2;
		msg.flags  = HAL_I2C_WR;
		if(hal_i2c_transfer(dev,&msg,1) < 0){
			return 1;
		}
		
		msg.buff   = buf;
		msg.length = 3;
		msg.flags  = HAL_I2C_RD;
		if(hal_i2c_transfer(dev,&msg,1) < 0){
			return 1;
		}
		temp = (short)(buf[0] << 8 | buf[1]);
		param->sensor_constants[i] = (float)temp;
	}
	
	return 0;
}

int32_t icm20789_i2c_recv_data(struct hal_dev_s *dev,uint8_t *buf,uint8_t len)
{
	struct hal_i2c_msg_s msg;
	
	msg.buff   = buf;
	msg.length = len;
	msg.flags  = HAL_I2C_RD;

	if(hal_i2c_transfer(dev,&msg,1) > 0){
		return 1;
	}
	return 0;
}


int icm20789_baro_init(struct hal_dev_s *dev)
{
	uint8_t buf[2];
	struct hal_i2c_dev_s *i2c_dev = (struct hal_i2c_dev_s *)dev;
	struct icm20789_baro_param_s *param = (struct icm20789_baro_param_s *)i2c_dev->user_data;
	
	icm20789_i2c_send_cmd(dev,ICM20789_BARO_READ_ID);
	icm20789_i2c_recv_data(dev,buf,2);
	
	if((buf[1] & 0x3F) != 0x08){
		return -1;
	}
	
	param->p_Pa_calib[0] = 45000.0;
	param->p_Pa_calib[1] = 80000.0;
	param->p_Pa_calib[2] = 105000.0;
	param->LUT_Lower     = 3.5 * (1 << 20);
	param->LUT_Upper     = 11.5 * (1 << 20);
	param->quadr_factor  = 1.0 / 16777216.0;
	param->offst_factor  = 2048.0;
	
	icm20789_i2c_read_param(dev);
	
	return 0;
}

int icm20789_baro_open(struct hal_dev_s *dev, uint16_t oflag)
{
	return 0;
}

void icm20789_baro_calculate_conversion_constants(struct icm20789_baro_param_s * s, float *p_Pa,
	float *p_LUT, float *out) {
	float A,B,C; C = (p_LUT[0] * p_LUT[1] * (p_Pa[0] - p_Pa[1]) +
	p_LUT[1] * p_LUT[2] * (p_Pa[1] - p_Pa[2]) +
	p_LUT[2] * p_LUT[0] * (p_Pa[2] - p_Pa[0])) / (p_LUT[2] * (p_Pa[0] - p_Pa[1]) +
	p_LUT[0] * (p_Pa[1] - p_Pa[2]) +
	p_LUT[1] * (p_Pa[2] - p_Pa[0]));
	A = (p_Pa[0] * p_LUT[0] - p_Pa[1] * p_LUT[1] - (p_Pa[1] - p_Pa[0]) * C) / (p_LUT[0] - p_LUT[1]);
	B = (p_Pa[0] - A) * (p_LUT[0] + C);
	out[0] = A;
	out[1] = B;
	out[2] = C; 
}

int icm20789_baro_process_data(struct icm20789_baro_param_s * s, int p_LSB, int T_LSB,
	float * pressure, float * temperature) {
	float t;
	float s1,s2,s3;
	float in[3];
	float out[3];
	float A,B,C; t = (float)(T_LSB - 32768);
	s1 = s->LUT_Lower + (float)(s->sensor_constants[0] * t * t) * s->quadr_factor;
	s2 = s->offst_factor * s->sensor_constants[3] + (float)(s->sensor_constants[1] * t * t) * s->quadr_factor;
	s3 = s->LUT_Upper + (float)(s->sensor_constants[2] * t * t) * s->quadr_factor;
	in[0] = s1;
	in[1] = s2;
	in[2] = s3;
	icm20789_baro_calculate_conversion_constants(s, s->p_Pa_calib, in, out);
	A = out[0];
	B = out[1];
	C = out[2];
	*pressure = A + B / (C + p_LSB);
	*temperature = -45.f + 175.f/65536.f * T_LSB;
	return 0;
}

int icm20789_baro_read(struct hal_dev_s *dev, void *buffer, int size,int pos)
{
	struct hal_i2c_dev_s *i2c_dev = (struct hal_i2c_dev_s *)dev;
	struct icm20789_baro_param_s *param = (struct icm20789_baro_param_s *)i2c_dev->user_data;
	struct baro_report_s *data = (struct baro_report_s *)buffer;
	uint8_t buff[9];
	int p_raw, t_raw;
	
	if(size != sizeof(struct baro_report_s)){
		return -1;
	}
	
	icm20789_i2c_recv_data(dev,buff,9);
	
	p_raw = ((uint32_t)(buff[0]) << 16) | ((uint32_t)(buff[1]) << 8) | buff[3];
	t_raw = ((uint32_t)(buff[6]) << 8) | buff[7];
	
	icm20789_baro_process_data(param,p_raw,t_raw,&data->pressure,&data->temperature);
	
	data->pressure *= 0.001f;
	
	return size;

}

int icm20789_baro_write(struct hal_dev_s *dev, const void *buffer, int size,int pos)
{
	return size;
}

int icm20789_baro_ioctl(struct hal_dev_s *dev, uint8_t cmd, void *args)
{
	int ret = 0;
	switch(cmd){
		case SENSOR_CMD_MEASURE:
			ret = icm20789_i2c_send_cmd(dev,ICM20789_BARO_MEASURE_PT_N);
			break;
		default:
			break;
	};
	return ret;
}

int icm20789_baro_close(struct hal_dev_s *dev)
{
	/* we can sleep mpu6050 here for saving power */
	return 0;
}

struct hal_i2c_dev_s            icm20789_baro;
struct icm20789_baro_param_s    icm20789_baro_param;
int icm20789_baro_register(void)
{
	icm20789_baro.port	    = 0;
	icm20789_baro.address   = 0x63;
	icm20789_baro.cfg.flags = HAL_I2C_WR | HAL_I2C_RD;
	icm20789_baro.cfg.speed = 400000;
	icm20789_baro.cfg.width = 8;

	icm20789_baro.dev.init  = icm20789_baro_init;
	icm20789_baro.dev.open  = icm20789_baro_open;
	icm20789_baro.dev.close = icm20789_baro_close;
	icm20789_baro.dev.read  = icm20789_baro_read;
	icm20789_baro.dev.write = icm20789_baro_write;
	icm20789_baro.dev.ioctl = icm20789_baro_ioctl;
	icm20789_baro.user_data = &icm20789_baro_param;

	hal_i2c_device_register(&icm20789_baro,"icm20789-baro",HAL_O_RDWR | HAL_DEV_STANDALONE);
	return 0;
};

